package my2Cents;

import java.awt.Color;
import java.awt.Graphics2D;

import net.phys2d.math.MathUtil;
import net.phys2d.math.Matrix2f;
import net.phys2d.math.ROVector2f;
import net.phys2d.math.Vector2f;
import net.phys2d.raw.BasicJoint;
import net.phys2d.raw.Body;
import net.phys2d.raw.FixedJoint;
import net.phys2d.raw.Joint;
import net.phys2d.raw.SpringJoint;

public class DrawingUtilities {
	/**
	 * Draw a joint 
	 * 
	 * @param g The graphics contact on which to draw
	 * @param j The joint to be drawn
	 */
	public static void drawCross(Graphics2D g, int x, int y, int size) {
		g.drawLine(x, y, x + size, y);
		g.drawLine(x, y, x - size, y);
		g.drawLine(x, y, x, y + size);
		g.drawLine(x, y, x, y - size);
	}
	
	public static void drawJoint(Graphics2D g, Joint j) {
		if (j instanceof FixedJoint) {
			FixedJoint joint = (FixedJoint) j;
			
			g.setColor(Color.red);
			float x1 = joint.getBody1().getPosition().getX();
			float x2 = joint.getBody2().getPosition().getX();
			float y1 = joint.getBody1().getPosition().getY();
			float y2 = joint.getBody2().getPosition().getY();
			
			g.drawLine((int) x1,(int) y1,(int) x2,(int) y2);
		}
		if (j instanceof BasicJoint) {
			BasicJoint joint = (BasicJoint) j;
			
			Body b1 = joint.getBody1();
			Body b2 = joint.getBody2();
	
			Matrix2f R1 = new Matrix2f(b1.getRotation());
			Matrix2f R2 = new Matrix2f(b2.getRotation());
	
			ROVector2f x1 = b1.getPosition();
			Vector2f p1 = MathUtil.mul(R1,joint.getLocalAnchor1());
			p1.add(x1);
	
			ROVector2f x2 = b2.getPosition();
			Vector2f p2 = MathUtil.mul(R2,joint.getLocalAnchor2());
			p2.add(x2);
	
			g.setColor(Color.red);
			//g.drawLine((int) x1.getX(), (int) x1.getY(), (int) p1.x, (int) p1.y);
			g.drawLine((int) p1.x, (int) p1.y, (int) x2.getX(), (int) x2.getY());
			//g.drawLine((int) x2.getX(), (int) x2.getY(), (int) p2.x, (int) p2.y);
			g.drawLine((int) p2.x, (int) p2.y, (int) x1.getX(), (int) x1.getY());
			drawCross(g, (int)p1.x, (int)p1.y, 5);
			drawCross(g, (int)p2.x, (int)p2.y, 5);
		}
		if (j instanceof SpringJoint) {
			SpringJoint joint = (SpringJoint) j;
			
			
			Body b1 = joint.getBody1();
			Body b2 = joint.getBody2();
	
			Matrix2f R1 = new Matrix2f(b1.getRotation());
			Matrix2f R2 = new Matrix2f(b2.getRotation());
	
			ROVector2f x1 = b1.getPosition();
			Vector2f p1 = MathUtil.mul(R1,joint.getLocalAnchor1());
			p1.add(x1);
	
			ROVector2f x2 = b2.getPosition();
			Vector2f p2 = MathUtil.mul(R2,joint.getLocalAnchor2());
			p2.add(x2);
			
			float ilength = j.getInitialLength();
			float length = j.getLength();
			float delta = (ilength - length) / ilength;
			if(joint.getLength() > joint.getMaxSpringSize()) {
				delta = Math.min(1, delta);
				delta = Math.max(0, delta);
			}
			delta = 1 - delta;
			
			float rawDist = p1.distance(p2);
			float dist = rawDist*delta;
			Vector2f normal = new Vector2f();
			normal.set(p2);
			normal.sub(p1);
			normal.normalise();
			
			Vector2f center = new Vector2f();
			center.set(normal);
			center.mul(rawDist/2);
			center.add(p1);
			
			Vector2f hp1 = new Vector2f();
			hp1.set(normal);
			hp1.mul(-dist/2);
			hp1.add(center);
		
			Vector2f hp2 = new Vector2f();
			hp2.set(normal);
			hp2.mul(dist/2);
			hp2.add(center);
			

			g.setColor(Color.red);
			
			//g.drawString("Length: " + j.getLength() + "/" + j.getInitialLength(), p1.x, p1.y);
			
			//g.drawLine((int) x1.getX(), (int) x1.getY(), (int) p1.x, (int) p1.y);
			drawCross(g, (int)p1.x, (int)p1.y, 5);
			drawCross(g, (int)p2.x, (int)p2.y, 5);
			drawCross(g, (int)center.x, (int)center.y, 2);
			g.drawLine((int) p1.x, (int) p1.y, (int) p2.getX(), (int) p2.getY());
			//g.drawLine((int) p2.getX(), (int) p2.getY(), (int) x2.getX(), (int) x2.getY());
			
			g.setColor(Color.gray);
			
			if(joint.getCompressedSpringConst() != 0) {
				g.setColor(Color.GREEN);
				if(delta < 0 || delta > 1) g.setColor(Color.red);
			}
			
			g.drawLine((int)hp1.x, (int)hp1.y, (int)hp2.x, (int)hp2.y);
//			g.drawLine((int)hp1.x, (int)hp1.y-1, (int)hp2.x, (int)hp2.y-1);
//			g.drawLine((int)hp1.x, (int)hp1.y+1, (int)hp2.x, (int)hp2.y+1);
//			g.drawLine((int)hp1.x-1, (int)hp1.y, (int)hp2.x-1, (int)hp2.y);
//			g.drawLine((int)hp1.x+1, (int)hp1.y, (int)hp2.x+1, (int)hp2.y);
		}
	}
}
